package multi.SAUNA;

import static java.awt.geom.AffineTransform.getRotateInstance;
import static java.awt.geom.AffineTransform.getTranslateInstance;

import java.awt.BasicStroke;
import java.awt.Color;
import java.awt.Graphics2D;
import java.awt.Polygon;
import java.awt.geom.AffineTransform;
import java.util.Vector;

import meta.symbolsAndTime.SAUNA.Map;
import org.metacsp.multi.spatial.rectangleAlgebra.Point;

public class Polyhedron {
	
	private Point p1, p2; 
	private Point pCenter;
	public Line[] spatialConstraints;
	public int[] directionOfGrowth;
	public Point[] points;
	private Color color;
	private double[] xyBounds;
	private boolean[] growable;
	private double[] maxGrowth;
	private Map map;
	private int forwardDirection;
	private int backwardDirection;
	private int[] sideDirections;
	private boolean invisible = false;
	private boolean last = false;
	
	public static double PRECISION = 0.01;
	
	public Polyhedron(Point p1, Point p2, double[] xyBounds, Map map) {
		this.map = map;
		this.xyBounds = xyBounds;
		this.color = Color.blue;
		this.p1 = p1;
		this.p2 = p2;
		createSpatialConstraints(p1,p2);
		calculatePoints();
		growable = new boolean[spatialConstraints.length];
		for (int i = 0; i < spatialConstraints.length; i++) {
			growable[i] = true;
			//growable[i] = false;
		}
		//growable[0] = true;
		maxGrowth = new double[spatialConstraints.length];
		for (int i = 0; i < maxGrowth.length; i++) maxGrowth[i] = Double.MAX_VALUE;
	}
	
	public Polyhedron(Point p1, Point p2, double[] xyBounds, Map map, double maxFwGrowth, double maxBkGrowth, double maxSideGrowth) {
		this(p1, p2, xyBounds, map);
		this.maxGrowth[forwardDirection] = maxFwGrowth;
		this.maxGrowth[backwardDirection] = maxBkGrowth;
		for (int dir : sideDirections) this.maxGrowth[dir] = maxSideGrowth;
	}
	
	public void setColor(Color c) { this.color = c; }
	
	public Color getColor() { return this.color; }
	
	private void calculatePoints() {
		points = new Point[spatialConstraints.length];
		for (int i = 0; i < spatialConstraints.length-1; i++) {
			points[i] = spatialConstraints[i].getIntersection(spatialConstraints[i+1]);	
		}
		points[points.length-1] = spatialConstraints[spatialConstraints.length-1].getIntersection(spatialConstraints[0]);
	}
	
	public void setInvisible(boolean invisible) {
		this.invisible = invisible;
	}
	
	private boolean hitsObstacle(int line) {
		//get adjacent lines
		int startLine = (line > 0) ? line-1 : spatialConstraints.length-1;
		int endLine = (line < spatialConstraints.length-1) ? line+1 : 0;
		Point start = spatialConstraints[line].getIntersection(spatialConstraints[startLine]);
		Point end = spatialConstraints[line].getIntersection(spatialConstraints[endLine]);
		Point[] sampledPoints = spatialConstraints[line].sample(start, end, PRECISION);
		for (Point p : sampledPoints) if (map.isObstacle(p)) return true;
		return false;
	}
	
	public boolean grow(double amount) {
		boolean grown = false;
		for (int i = 0; i < growable.length; i++) {
			if (growable[i] && maxGrowth[i] > 0) {
				Line oldLine = spatialConstraints[i];
				Line newLine = spatialConstraints[i].getTranslation(directionOfGrowth[i]*Math.abs(amount));
				spatialConstraints[i] = newLine;
				//check if any of the changing lines hit obstacle
				//int adjacentLine1 = (i > 0) ? i-1 : spatialConstraints.length-1;
				//int adjacentLine2 = (i < spatialConstraints.length-1) ? i+1 : 0;
				if (hitsObstacle(i) /*|| hitsObstacle(adjacentLine1,precision) || hitsObstacle(adjacentLine2,precision)*/) {
					growable[i] = false;
					spatialConstraints[i] = oldLine;
				}
				else {
					grown = true;
					maxGrowth[i] -= Math.abs(amount);
				}
			}
		}
		
		if (grown) {
			calculatePoints();
			//System.out.println("Grown...");
		}
		return grown;
	}
	
	private void createSpatialConstraints(Point p1, Point p2) {
		spatialConstraints = new Line[4];
		directionOfGrowth = new int[4];
		
		pCenter = new Point(p1.x+(p2.x-p1.x)/2,p1.y+(p2.y-p1.y)/2);
		
		//line thru points
		Line parallel = new Line(p1, p2);
		
		//lines parallel to line thru points (up and down)
		spatialConstraints[1] = parallel.getTranslation(xyBounds[1]);
		directionOfGrowth[1] = 1;
		spatialConstraints[3] = parallel.getTranslation(-xyBounds[1]);
		directionOfGrowth[3] = -1;
		
		//line perpendicular to line thru points
		Line perpendicular = parallel.getPerpendicular(pCenter);
		
		//lines parallel to perpendicular (left and right)
		spatialConstraints[0] = perpendicular.getTranslation(-xyBounds[0]);
		directionOfGrowth[0] = -1;
		spatialConstraints[2] = perpendicular.getTranslation(xyBounds[0]);
		directionOfGrowth[2] = 1;
		
		//Calc fw direction
		Point fwPoint = parallel.getIntersection(spatialConstraints[0]);
		if (p2.distance(fwPoint) < p1.distance(fwPoint)) {
			forwardDirection = 0;
			backwardDirection = 2;
		}
		else {
			forwardDirection = 2;
			backwardDirection = 0;
		}
		sideDirections = new int[] {1,3};
	}
	
	public boolean isInvisible() {
		return invisible;
	}
	
	public void setLast(boolean last) {
		this.last = last;
	}
	
	public boolean isLast() {
		return last;
	}
	
	public Polyhedron clone() {
		Polyhedron cp = new Polyhedron(p1, p2, xyBounds, map);
		cp.pCenter = pCenter;
		cp.spatialConstraints = spatialConstraints;
		cp.directionOfGrowth = directionOfGrowth;
		cp.points = points;
		cp.color = color;
		cp.xyBounds = xyBounds;
		cp.growable = growable;
		cp.maxGrowth = maxGrowth;
		cp.map = map;
		cp.forwardDirection = forwardDirection;
		cp.backwardDirection = backwardDirection;
		cp.sideDirections = sideDirections;
		cp.invisible = invisible;
		cp.last = last;
		return cp;
	}
	
	public void draw(Graphics2D g, String name) {
		if (!this.invisible) {
			g.setStroke(new BasicStroke(2.0f));
			g.setColor(color);
			int[] xCoords = new int[points.length];
			int[] yCoords = new int[points.length];
			for (int i = 0; i < points.length; i++) {
				Point screen = map.toScreen(points[i].x,points[i].y);
				xCoords[i] = screen.x();
				yCoords[i] = screen.y();
			}
			Polygon p = new Polygon(xCoords, yCoords, points.length);
			if (this.getColor().equals(Color.blue)) g.draw(p);
			else g.fillPolygon(p);
			
//			Point centerPoint = map.toScreen(this.pCenter.x, this.pCenter.y);
//			g.setFont(Font.decode("Arial-BOLD-12"));
//			g.setColor(Color.black);
//			g.drawString(name, centerPoint.x(), centerPoint.y());

			g.setColor(Color.black);
			paintArrow(g,map.toScreen(p1.x,p1.y),map.toScreen(p2.x,p2.y));
		}
	}
			
	private void paintArrow(Graphics2D g, Point p1, Point p2) {
		int ARR_LEN = 6, ARR_WIDTH = 4;
        double dx = p2.x - p1.x, dy = p2.y - p1.y;
        double angle = Math.atan2(dy, dx);
        int len = (int) Math.sqrt(dx*dx + dy*dy);
        
        AffineTransform saveAT = g.getTransform();
        AffineTransform at = getTranslateInstance(p1.x, p1.y);
        at.concatenate(getRotateInstance(angle));
        g.setTransform(at);

        // Draw horizontal arrow starting in (0, 0)
        g.drawLine(0, 0, (int) len, 0);
        g.fillPolygon(new int[] {len, len-ARR_LEN, len-ARR_LEN, len},
                      new int[] {0, -ARR_WIDTH, ARR_WIDTH, 0}, 4);
        
        g.setTransform(saveAT);
    }
	
	public boolean contains(Point p) {
		int[] xCoords = new int[points.length];
		int[] yCoords = new int[points.length];
		for (int i = 0; i < points.length; i++) {
			xCoords[i] = new Double(points[i].x/PRECISION).intValue();
			yCoords[i] = new Double(points[i].y/PRECISION).intValue();
		}
		Polygon poly = new Polygon(xCoords, yCoords, points.length);
		return poly.contains(p.x/PRECISION, p.y/PRECISION);
	}
	
	public Point[] getPoints() { return points; }
	
	public Line getForwarwardDirection() { return spatialConstraints[forwardDirection]; }
	
	public boolean intersectsForwardConstraint(Line l, Point inside) {
		double[] distances = new double[spatialConstraints.length];
		for (int i = 0; i < spatialConstraints.length; i++) {
			distances[i] = l.getIntersection(spatialConstraints[i]).distance(inside);
		}
		int minIndex = -1;
		double min = Double.MAX_VALUE;
		for (int i = 0; i < distances.length; i++) {
			if ( !Double.isNaN(distances[i]) && !Double.isInfinite(distances[i]) && distances[i] < min ) {
				min = distances[i];
				minIndex = i;
			}
		}
		return (minIndex == forwardDirection);
	}
	
	public boolean intersects(Polyhedron p) {
		int[] xCoords1 = new int[points.length];
		int[] yCoords1 = new int[points.length];
		for (int i = 0; i < points.length; i++) {
			xCoords1[i] = new Double(points[i].x/PRECISION).intValue();
			yCoords1[i] = new Double(points[i].y/PRECISION).intValue();
		}
		Polygon poly1 = new Polygon(xCoords1, yCoords1, this.getPoints().length);
		int[] xCoords2 = new int[p.getPoints().length];
		int[] yCoords2 = new int[p.getPoints().length];
		for (int i = 0; i < points.length; i++) {
			xCoords2[i] = new Double(p.getPoints()[i].x/PRECISION).intValue();
			yCoords2[i] = new Double(p.getPoints()[i].y/PRECISION).intValue();
		}
		Polygon poly2 = new Polygon(xCoords2, yCoords2, p.getPoints().length);
		boolean containsPoint = false;
		for (int i = 0; i < xCoords1.length && !containsPoint; i++) {
			if (poly2.contains(xCoords1[i], yCoords1[i])) containsPoint = true;
		}
		if (containsPoint) return true;
		for (int i = 0; i < xCoords2.length && !containsPoint; i++) {
			if (poly1.contains(xCoords2[i], yCoords2[i])) containsPoint = true;
		}
		if (containsPoint) return true;
		
		//could still be special case
		Line[] constraints1 = this.spatialConstraints;
		Line[] constraints2 = p.spatialConstraints;
	
		for (Line l1 : constraints1) {
			for (Line l2 : constraints2) {
				Point point = l1.getIntersection(l2);
				if (this.contains(point) && p.contains(point)) {
					//System.out.println("Is " + this + " INTERSECT " + p + " a special case?");
					return true;
				}
			}
		}

		return false;
	}
	
	public Line getForwardConstraint() {
		return this.spatialConstraints[forwardDirection];
	}

	public Line getBackwardConstraint() {
		return this.spatialConstraints[backwardDirection];
	}

	public Line[] getSideConstraints() {
		Line[] ret = new Line[spatialConstraints.length-2];
		for (int i = 0; i < spatialConstraints.length; i++) {
			if (i != forwardDirection && i != backwardDirection) ret[i] = spatialConstraints[i];
		}
		return ret;
	}

	public Line[] getForwardAndSideConstraints() {
		Vector<Line> ret = new Vector<Line>();
		for (int i = 0; i < spatialConstraints.length; i++) {
			if (i != backwardDirection) ret.add(spatialConstraints[i]);
		}
		return ret.toArray(new Line[ret.size()]);
	}
	
}